﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ARCam3D.Tuple;

namespace ARCam3D.Imaging.Processing
{
    public class ContourChecker
    {
        private double pointsCoveringFactor = 0.85;
        private double distanceThreshold = 6;//in pixels, todo make relative
        private int boundsThreshold = 6;

        public bool CheckContour(List<Point2I> points, Point2I[] corners)
        {
            if (corners.Length < 1)
            {
                return false;
            }

            int passedCount = 0;
            foreach (Point2I point in points)
            {
                double minDistance = double.MaxValue;
                for (int i = 0; i < corners.Length; ++i)
                {
                    Point2I p1 = corners[i];
                    Point2I p2 = corners[(i + 1) % corners.Length];

                    if (point.x - Math.Max(p1.x, p2.x) < boundsThreshold &&
                        Math.Min(p1.x, p2.x) - point.x < boundsThreshold &&
                        point.y - Math.Max(p1.y, p2.y) < boundsThreshold &&
                        Math.Min(p1.y, p2.y) - point.y < boundsThreshold)
                    {
                        double distance = getLinePointDistance(p1, p2, point);
                        if (distance < minDistance)
                        {
                            minDistance = distance;
                        }
                    }
                }

                if (minDistance < distanceThreshold)
                {
                    ++passedCount;
                }
            }

            if (((double)passedCount) / points.Count > pointsCoveringFactor)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        private double getLinePointDistance(Point2I linePoint1, Point2I linePoint2, Point2I point)
        {
            int dy = linePoint1.y - linePoint2.y;
            int dx = linePoint1.x - linePoint2.x;

            double distance = point.x * dy - point.y * dx + linePoint1.x * linePoint2.y - linePoint1.y * linePoint2.x;
            distance = distance * distance;
            distance = distance / (dx * dx + dy * dy);
            distance = Math.Sqrt(distance);

            return distance;
        }
    }
}
